package org.gunncs.actors;
import org.gunncs.actoriface.*;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import java.util.*;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.iaroc.irobot.IRobotCreate;
import com.iaroc.irobot.util.*;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;
import com.sun.spot.sensorboard.peripheral.Servo;

/**
 * <<Class summary>>
 *
 * @author Anand Gupta &lt;&gt;
 * @version $Rev$
 */
public class CrashActor implements Actor{
  
    public static final int BACKUPDIST = -13;
    public static final int ADJUST_ANGLE = 14;
    public static final int ROT_ANGLE = 25;
    
    private double lastLeft;
    private double lastRight;
    private double leftStdev;
    private double rightStdev;
    boolean leftBump;
    boolean rightBump;
    ITriColorLED overallStatus = EDemoBoard.getInstance().getLEDs()[3];
    public CrashActor(){
      leftBump = rightBump = false;
      rightStdev =0;
      leftStdev = 0;
      lastLeft = 0;
      lastRight = 0;
    }

    public void act(Robot bot){
    	System.out.println(this +" acting");
    	if(leftBump || rightBump){
    	  bot.goBackward();
        bot.waitDistance(BACKUPDIST);
               
    	}
      if(leftBump && rightBump){
        overallStatus.setColor(LEDColor.RED);
        overallStatus.setOn();
          Random ran = new Random();
          if(ran.nextDouble()<.5) 
            bot.turnLeft(ROT_ANGLE);
          else bot.turnRight(ROT_ANGLE);
      }else if(leftBump){
        bot.turnRight(ADJUST_ANGLE);
        overallStatus.setColor(LEDColor.YELLOW);
        overallStatus.setOn();
        bot.goForward();
        bot.waitDistance((int)(BACKUPDIST*1.4142));        
      }else if(rightBump){
        bot.turnLeft(ADJUST_ANGLE);
        overallStatus.setColor(LEDColor.ORANGE);
        overallStatus.setOn();
        bot.goForward();
        bot.waitDistance((int)(BACKUPDIST*1.4142));
      }else
        overallStatus.setOff();
    }
    
    public void updateSensors(Sensor[] sensors){
      lastLeft = sensors[2].getState()[0];
      lastRight = sensors[4].getState()[0];
      leftStdev = sensors[2].getState()[1];
      rightStdev = sensors[4].getState()[1];
      leftBump = (sensors[0].getState()[BumpSensor.LEFT] == BumpSensor.PRESSED);
      rightBump = (sensors[0].getState()[BumpSensor.RIGHT] == BumpSensor.PRESSED);
    }
}
